#include "CAN_receive.h"
#include "main.h"
#include "detect_task.h"

extern CAN_HandleTypeDef hcan1;
extern CAN_HandleTypeDef hcan2;

#define get_mit_motor_measure(ptr, data)														\
	{																							\
		(ptr)->id = (data[0]) & 0x0F;															\
		(ptr)->p_int = (data[1] << 8) | data[2];												\
		(ptr)->v_int = (data[3] << 4) | (data[4] >> 4);											\
		(ptr)->t_int = ((data[4] & 0xF) << 8) | data[5];										\
		(ptr)->pos = uint_to_float((ptr)->p_int, P_MIN, P_MAX, 16);								\
		(ptr)->vel = uint_to_float((ptr)->v_int, V_MIN, V_MAX, 12);								\
		(ptr)->tor = uint_to_float((ptr)->t_int, T_MIN, T_MAX, 12);								\
		(ptr)->Temp = (fp32)(rx_data[6]);														\
		(ptr)->err_state = (fp32)(rx_data[7]);													\
	}

#define get_motor_measure(ptr, data)									\
	{																	\
		(ptr)->last_ecd = (ptr)->ecd;                                   \
		(ptr)->ecd = (uint16_t)((data)[0] << 8 | (data)[1]);            \
		(ptr)->speed_rpm = (uint16_t)((data)[2] << 8 | (data)[3]);      \
		(ptr)->given_current = (uint16_t)((data)[4] << 8 | (data)[5]);  \
		(ptr)->temperate = (data)[6];                                   \
	}

#define get_supercap_measure(ptr, data)											\
	{																			\
		(ptr)->battery_voltage = (uint16_t)((data)[0] << 8 | (data)[1]);		\
		(ptr)->chassis_power = ((uint16_t)((data)[2] << 8 | (data)[3])) / 100;	\
		(ptr)->given_current = (uint16_t)((data)[4] << 8 | (data)[5]);			\
		(ptr)->capacitance_percentage = (data)[6];								\
		(ptr)->power_set = (data)[7];											\
	}

void canx_send_data(CAN_HandleTypeDef *hcan, uint16_t id, uint8_t *data);
static uint16_t float_to_uint(fp32 x_float, fp32 x_min, fp32 x_max, uint16_t bits);
static fp32 uint_to_float(uint16_t x_int, fp32 x_min, fp32 x_max, uint16_t bits);

mit_motor_measure_t mit_motor_gimbal[1];
motor_measure_t motor_chassis[4];
motor_measure_t motor_gimbal[5];
sup_measure_t sup_cap[1];

static CAN_TxHeaderTypeDef  gimbal_tx_message;
static uint8_t              gimbal_can_send_data[8];
static CAN_TxHeaderTypeDef  chassis_tx_message;
static uint8_t              chassis_can_send_data[8];
static CAN_TxHeaderTypeDef  shoot_tx_message;
static uint8_t              shoot_can_send_data[8];
static CAN_TxHeaderTypeDef  sup_tx_message;
static uint8_t              sup_can_send_data[2];

void enable_radar_yaw_motor_mode(CAN_HandleTypeDef* hcan, uint16_t motor_id, uint16_t mode_id)
{
	uint8_t data[8];
	uint16_t id = motor_id + mode_id;

	data[0] = 0xFF;
	data[1] = 0xFF;
	data[2] = 0xFF;
	data[3] = 0xFF;
	data[4] = 0xFF;
	data[5] = 0xFF;
	data[6] = 0xFF;
	data[7] = 0xFC;

	canx_send_data(hcan, id, data);
}

void init_radar_yaw_motor_mode(CAN_HandleTypeDef* hcan, uint16_t motor_id, uint16_t mode_id)
{
	uint8_t data[8];
	uint16_t id = motor_id + mode_id;

	data[0] = 0xFF;
	data[1] = 0xFF;
	data[2] = 0xFF;
	data[3] = 0xFF;
	data[4] = 0xFF;
	data[5] = 0xFF;
	data[6] = 0xFF;
	data[7] = 0xFE;

	canx_send_data(hcan, id, data);
}

void disable_radar_yaw_motor_mode(CAN_HandleTypeDef *hcan, uint16_t motor_id, uint16_t mode_id)
{
	uint8_t data[8];
	uint16_t id = motor_id + mode_id;
	data[0] = 0xFF;
	data[1] = 0xFF;
	data[2] = 0xFF;
	data[3] = 0xFF;
	data[4] = 0xFF;
	data[5] = 0xFF;
	data[6] = 0xFF;
	data[7] = 0xFD;

	canx_send_data(hcan, id, data);
}

void canx_send_data(CAN_HandleTypeDef *hcan, uint16_t id, uint8_t *data)
{
	CAN_TxHeaderTypeDef TxHeader;

	uint32_t send_mail_box;
	TxHeader.StdId = id; // CAN ID
	TxHeader.IDE = CAN_ID_STD;
	TxHeader.RTR = CAN_RTR_DATA;
	TxHeader.DLC = 0x08;;
	HAL_CAN_AddTxMessage(hcan, &TxHeader, data, &send_mail_box);
}

void radar_yaw_motor_init(void)
{
	mit_motor_gimbal[0].mode = MIT_MODE;
	mit_motor_gimbal[0].id = 1;
}

void mit_ctrl_big_yaw_motor(CAN_HandleTypeDef* hcan, uint16_t motor_id, fp32 pos, fp32 vel, fp32 kp, fp32 kd, fp32 torq)
{
	uint8_t data[8];
	uint16_t pos_tmp, vel_tmp, kp_tmp, kd_tmp, tor_tmp;
	uint16_t id = motor_id + MIT_MODE;

	pos_tmp = float_to_uint(pos, P_MIN, P_MAX, 16);
	vel_tmp = float_to_uint(vel, V_MIN, V_MAX, 12);
	kp_tmp = float_to_uint(kp, KP_MIN, KP_MAX, 12);
	kd_tmp = float_to_uint(kd, KD_MIN, KD_MAX, 12);
	tor_tmp = float_to_uint(torq, T_MIN, T_MAX, 12);

	data[0] = (pos_tmp >> 8);
	data[1] = pos_tmp;
	data[2] = (vel_tmp >> 4);
	data[3] = ((vel_tmp & 0xF) << 4) | (kp_tmp >> 8);
	data[4] = kp_tmp;
	data[5] = (kd_tmp >> 4);
	data[6] = ((kd_tmp & 0xF) << 4) | (tor_tmp >> 8);
	data[7] = tor_tmp;
	
	canx_send_data(hcan, id, data);
}

void CAN_cmd_gimbal(int16_t yaw ,int16_t pitch, int16_t fric ,int16_t rev)
{
    uint32_t send_mail_box;
    gimbal_tx_message.StdId = CAN_GIMBAL_ALL_ID;
    gimbal_tx_message.IDE = CAN_ID_STD;
    gimbal_tx_message.RTR = CAN_RTR_DATA;
    gimbal_tx_message.DLC = 0x08;

    gimbal_can_send_data[0] = yaw >> 8;
    gimbal_can_send_data[1] = yaw;
    gimbal_can_send_data[2] = pitch >> 8;
    gimbal_can_send_data[3] = pitch;
    gimbal_can_send_data[4] = fric >> 8;
    gimbal_can_send_data[5] = fric;
    gimbal_can_send_data[6] = rev >> 8;
    gimbal_can_send_data[7] = rev;

    HAL_CAN_AddTxMessage(&GIMBAL_CAN, &gimbal_tx_message, gimbal_can_send_data, &send_mail_box);
}

void CAN_cmd_chassis_reset_ID(void)
{
    uint32_t send_mail_box;
    chassis_tx_message.StdId = 0x700;
    chassis_tx_message.IDE = CAN_ID_STD;
    chassis_tx_message.RTR = CAN_RTR_DATA;
    chassis_tx_message.DLC = 0x08;
    chassis_can_send_data[0] = 0;
    chassis_can_send_data[1] = 0;
    chassis_can_send_data[2] = 0;
    chassis_can_send_data[3] = 0;
    chassis_can_send_data[4] = 0;
    chassis_can_send_data[5] = 0;
    chassis_can_send_data[6] = 0;
    chassis_can_send_data[7] = 0;

    HAL_CAN_AddTxMessage(&CHASSIS_CAN, &chassis_tx_message, chassis_can_send_data, &send_mail_box);
}

void CAN_cmd_chassis(int16_t motor1, int16_t motor2, int16_t motor3, int16_t motor4)
{
    uint32_t send_mail_box;
    chassis_tx_message.StdId = CAN_CHASSIS_ALL_ID;
    chassis_tx_message.IDE = CAN_ID_STD;
    chassis_tx_message.RTR = CAN_RTR_DATA;
    chassis_tx_message.DLC = 0x08;
    chassis_can_send_data[0] = motor1 >> 8;
    chassis_can_send_data[1] = motor1;
    chassis_can_send_data[2] = motor2 >> 8;
    chassis_can_send_data[3] = motor2;
    chassis_can_send_data[4] = motor3 >> 8;
    chassis_can_send_data[5] = motor3;
    chassis_can_send_data[6] = motor4 >> 8;
    chassis_can_send_data[7] = motor4;

    HAL_CAN_AddTxMessage(&CHASSIS_CAN, &chassis_tx_message, chassis_can_send_data, &send_mail_box);
}

void CAN_cmd_shoot(int16_t shoot1 ,int16_t shoot2, int16_t rev1, int16_t rev2)
{ 
    uint32_t send_mail_box;
    shoot_tx_message.StdId = CAN_SHOOT_ALL_ID;
    shoot_tx_message.IDE = CAN_ID_STD;
    shoot_tx_message.RTR = CAN_RTR_DATA;
    shoot_tx_message.DLC = 0x08;
    shoot_can_send_data[0] = shoot1 >> 8;
    shoot_can_send_data[1] = shoot1;
    shoot_can_send_data[2] = shoot2 >> 8;
    shoot_can_send_data[3] = shoot2;
    shoot_can_send_data[4] = rev1 >> 8;
    shoot_can_send_data[5] = rev1;
    shoot_can_send_data[6] = rev2 >> 8;
    shoot_can_send_data[7] = rev2;
    HAL_CAN_AddTxMessage(&SHOOT_CAN, &shoot_tx_message, shoot_can_send_data, &send_mail_box);
}

void CAN_cmd_supercap(int16_t power_set)
{
  uint32_t send_mail_box;
	power_set = power_set *100;
  sup_tx_message.StdId = CAN_SUPERCAP_TX_ID;
  sup_tx_message.IDE = CAN_ID_STD;
  sup_tx_message.RTR = CAN_RTR_DATA;
  sup_tx_message.DLC = 0x02;
  sup_can_send_data[0] = power_set >> 8;
  sup_can_send_data[1] = power_set;
  HAL_CAN_AddTxMessage(&CHASSIS_CAN, &sup_tx_message, sup_can_send_data, &send_mail_box);
}

void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan)
{
	CAN_RxHeaderTypeDef rx_header;
	uint8_t rx_data[8] = {0};

	if(hcan == &CHASSIS_CAN)
	{
		HAL_CAN_GetRxMessage(&CHASSIS_CAN, CAN_RX_FIFO0, &rx_header, rx_data);
		switch (rx_header.StdId)
		{
			case CAN_3508_M1_ID:
			case CAN_3508_M2_ID:
			case CAN_3508_M3_ID:
			case CAN_3508_M4_ID:
			{
				static uint8_t i = 0;
				i = rx_header.StdId - CAN_3508_M1_ID;
				get_motor_measure(&motor_chassis[i], rx_data);
				detect_hook(CHASSIS_MOTOR1_TOE + i);
				break;
			}
			case 0x00 :
			{
				get_mit_motor_measure(&mit_motor_gimbal[0], rx_data);
				break;
			}
			case CAN_SUPERCAP_RX_ID:
			{
				get_supercap_measure(&sup_cap[0], rx_data);
				break;
			}

			default:
			{
				break;
			}
		}
	}
	else if(hcan == &GIMBAL_CAN)
	{
		HAL_CAN_GetRxMessage(&GIMBAL_CAN, CAN_RX_FIFO0, &rx_header, rx_data);
		switch (rx_header.StdId)
		{
			case CAN_YAW_MOTOR_ID:
			case CAN_PIT_MOTOR_ID:
			case CAN_TRIGER_MOTOR_ID:
			{
				static uint8_t i = 0;
				i = rx_header.StdId - CAN_YAW_MOTOR_ID;
				get_motor_measure(&motor_gimbal[i], rx_data);
				detect_hook(YAW_GIMBAL_MOTOR_TOE + i);
				break;
			}
			case CAN_FRIC1_ID:
			case CAN_FRIC2_ID:
			{
				static uint8_t j = 0;
				j = rx_header.StdId - CAN_SHOOT_ALL_ID + 2;
				get_motor_measure(&motor_gimbal[j], rx_data);
				detect_hook(YAW_GIMBAL_MOTOR_TOE + j);
				break;
			}
			default:
			{
				break;
			}
		}
	}
}

const mit_motor_measure_t *get_yaw_gimbal_mit_motor_measure_point(void)
{
	return &mit_motor_gimbal[0];
}

const motor_measure_t *get_yaw_gimbal_motor_measure_point(void)
{
    return &motor_gimbal[0];
}

const motor_measure_t *get_pitch_gimbal_motor_measure_point(void)
{
    return &motor_gimbal[1];
}

const motor_measure_t *get_trigger_motor_measure_point(void)
{
    return &motor_gimbal[2];
}

const motor_measure_t *get_chassis_motor_measure_point(uint8_t i)
{
    return &motor_chassis[(i & 0x03)];
}

const motor_measure_t *get_fric1_motor_measure_point(void)
{
  return &motor_gimbal[3];
}

const motor_measure_t *get_fric2_motor_measure_point(void)
{
  return &motor_gimbal[4];
}

const sup_measure_t *get_supercap_measure_point(void)
{
  return &sup_cap[0];
}

uint16_t float_to_uint(fp32 x_float, fp32 x_min, fp32 x_max, uint16_t bits)
{
		fp32 span = x_max - x_min;
		fp32 offset = x_min;
		return (uint16_t) ((x_float - offset) * ((fp32)((1 << bits) - 1)) / span);
}

fp32 uint_to_float(uint16_t x_int, fp32 x_min, fp32 x_max, uint16_t bits)
{
		fp32 span = x_max - x_min;
		fp32 offset = x_min;
		return ((fp32)x_int) * span / ((fp32)((1 << bits) - 1)) + offset;
}
